/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
/*----------------------------------------------------------------------------*/
#ifndef PIDCONTROLLER_H_
#define PIDCONTROLLER_H_

class PIDOutput;
class PIDSource;

/**
 * Class implements a PID Control Loop.
 * 
 * Creates a separate thread which reads the given PIDSource and takes
 * care of the integral calculations, as well as writing the given
 * PIDOutput
 */
class PIDController
{
private:
	float P_;			// factor for "proportional" control
	float I_;			// factor for "integral" control
	float D_;			// factor for "derivative" control
	float maximumOutput_;	// |maximum output|
	float minimumOutput_;	// |minimum output|
	
	long maximumInput_;		// maximum input - limit setpoint to this
	long minimumInput_;		// minimum input - limit setpoint to this
	
	bool continuous_;		// do the endpoints wrap around? eg. Absolute encoder
	
	bool enabled_; 			//is the pid controller enabled
	
	long prevError_;	// the prior sensor input (used to compute velocity)
	
	long long totalError_; //the sum of the errors for use in the integral calc
	
	float tolerence_;	//the percetage error that is considered on target
	
	long setpoint_;
	
	long error_;
	
	float result_;
	
	PIDSource *pidInput_;
	PIDOutput *pidOutput_;

	static void Calculate(void *controller);

	Notifier *controlLoop_;
	
public:
	PIDController(float p, float i, float d);
	~PIDController(void);
	float Get(void);
	void SetContinuous(void);
	void SetContinuous(bool continuous);
	void SetInput(PIDSource *pidInput);
	void SetInput(PIDSource *pidInput, long minimumInput, long maximumInput);
	void SetOutput(PIDOutput *pidOutput);
	void SetOutput(PIDOutput *pidOutput, float mimimumOutput, float maximumOutput);
	
	void SetSetpoint(long setpoint);
	long GetSetpoint(long setpoint);

	long GetError(void);
	
	void SetTolerence(float percent);
	bool OnTarget(void);
	
	void Enable(void);
	void Disable(void);
	
	void Reset(void);
};

#endif
